/*
 *  Copyright (C) 2004 Steve Harris, Uwe Koloska
 *
 *  This program is free software; you can redistribute it and/or modify
 *  it under the terms of the GNU General Public License as published by
 *  the Free Software Foundation; either version 2 of the License, or
 *  (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU General Public License for more details.
 *
 *  $Id: example_server.c,v 1.2 2005/04/25 21:46:46 xovo Exp $
 */

#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <pthread.h>

#include "lo/lo.h"
#include "Vector3.h"

#define UPDATE_PATH "/infinity/update"
#define INFINITY_PORT "25669"

int done = 0;
int updates = 0;

int directenabled = 0;
int pathenabled = 0;
int pathprogress = 0;

int padenabled = 0;
int speed = 0;
int forward = 0;
int backward = 0;
int up = 0;
int down = 0;
int left = 0;
int right = 0;
int forwardcommands = 0;
int backwardcommands = 0;
int upcommands = 0;
int downcommands = 0;
int leftcommands = 0;
int rightcommands = 0;



int stationnumber = 0;
int stationsenabled = 0;
int atstation = 0;
int atstationnumber = 0;

int navigatingtostation = 0;
int distancetraveled = 0;


Vector3 position;
Vector3 target;
Vector3 storedtarget;

Vector3 path[5];

Vector3 stations[5];

lo_address infinitydriver;

lo_address ipad;

lo_address lightcontrol;

void error(int num, const char *m, const char *path);

int generic_handler(const char *path, const char *types, lo_arg **argv,
		    int argc, void *data, void *user_data);

int update_handler(const char *path, const char *types, lo_arg **argv,
		    int argc, void *data, void *user_data);

int lightson_handler(const char *path, const char *types, lo_arg **argv,
		    int argc, void *data, void *user_data);

int lightsoff_handler(const char *path, const char *types, lo_arg **argv,
		    int argc, void *data, void *user_data);

int depth_handler(const char *path, const char *types, lo_arg **argv,
		    int argc, void *data, void *user_data);

int direction_handler(const char *path, const char *types, lo_arg **argv,
		    int argc, void *data, void *user_data);

int emergency_handler(const char *path, const char *types, lo_arg **argv, int argc,
		 void *data, void *user_data);

int direct_handler(const char *path, const char *types, lo_arg **argv, int argc,
		 void *data, void *user_data);

int save_handler(const char *path, const char *types, lo_arg **argv, int argc,
		 void *data, void *user_data);

int go_handler(const char *path, const char *types, lo_arg **argv, int argc,
		 void *data, void *user_data);

int path_handler(const char *path, const char *types, lo_arg **argv, int argc,
		 void *data, void *user_data);

int clear_state_handler(const char *path, const char *types, lo_arg **argv, int argc,
		 void *data, void *user_data);

int load_state_handler(const char *path, const char *types, lo_arg **argv, int argc,
		 void *data, void *user_data);

int playback_handler(const char *path, const char *types, lo_arg **argv, int argc,
		 void *data, void *user_data);

int speed_handler(const char *path, const char *types, lo_arg **argv, int argc,
		 void *data, void *user_data);

int station_handler(const char *path, const char *types, lo_arg ** argv, int argc,
		void *data, void *user_data);

void * target_thread(void * arg);

void * target_set_thread(void * arg);

void * direction_reset_thread(void * arg);

void * target_move_thread(void * arg);

pthread_mutex_t mutex = PTHREAD_MUTEX_INITIALIZER;

int main()
{
  target.x = 0;
  target.y = 0;
  target.z = 0;

  position.x = 0;
  position.y = 0;
  position.z = 0;

  path[0].x = 5100;
  path[0].y = 6077;
  path[0].z = 2000;

  path[1].x = 5100;
  path[1].y = 6077;
  path[1].z = 5400;

  path[2].x = 0;
  path[2].y = 3000;
  path[2].z = 6250;

  path[3].x = 0;
  path[3].y = 4000;
  path[3].z = 7400;

  path[4].x = 0;
  path[4].y = 5700;
  path[4].z = 8500;

  stations[0].x = 5203;
  stations[0].y = 6077;
  stations[0].z = 5978;

  //front left
  stations[1].x = 2717;
  stations[1].y = 6077;
  stations[1].z = 2267;

  //front right
  stations[2].x = 2717;
  stations[2].y = 6077;
  stations[2].z = 9689;

  //back left
  stations[3].x = 7690;
  stations[3].y = 6077;
  stations[3].z = 2267;

  //back right
  stations[4].x = 7690;
  stations[4].y = 6077;
  stations[4].z = 9689;





	infinitydriver = lo_address_new("192.168.2.89", INFINITY_PORT);

  ipad = lo_address_new("192.168.2.69", "1025");

  lightcontrol = lo_address_new("192.168.2.70", "26000");

  /* start a new server on port 7770 */
  lo_server_thread st = lo_server_thread_new("25670", error);

  /* add method that will match any path and args */
  //lo_server_thread_add_method(st, NULL, NULL, generic_handler, NULL);

  lo_server_thread_add_method(st, UPDATE_PATH, "fff", update_handler, NULL);

  lo_server_thread_add_method(st, "/push", "f", direction_handler, NULL);

  lo_server_thread_add_method(st, "/stop", "f", emergency_handler, NULL);

  lo_server_thread_add_method(st, "/direct", "ff", direct_handler, NULL);

  lo_server_thread_add_method(st, "/directdepth", "f", depth_handler, NULL);

  lo_server_thread_add_method(st, "/save", "f", save_handler, NULL);

  lo_server_thread_add_method(st, "/go", "f", go_handler, NULL);

  lo_server_thread_add_method(st, "/lightson", "f", path_handler, NULL);

  lo_server_thread_add_method(st, "/loadState", NULL, load_state_handler, NULL);

  lo_server_thread_add_method(st, "/clearState", NULL, clear_state_handler, NULL);

  lo_server_thread_add_method(st, "/playback", "i", playback_handler, NULL);

  lo_server_thread_add_method(st, "/path", "f", path_handler, NULL);

  lo_server_thread_add_method(st, "/speed", "f", speed_handler, NULL);

  //lo_server_thread_add_method(st, "/lightson", "f", lightson_handler, NULL);

  lo_server_thread_add_method(st, "/lightsoff", "f", lightsoff_handler, NULL);

  lo_server_thread_add_method(st, "/station", "i", station_handler, NULL);

  lo_server_thread_start(st);

	pthread_t tid[4];
	int rc;

	rc = pthread_create( &tid[0], NULL, target_thread, NULL );
	if ( rc != 0 ) {
		perror( "Could not create thread" );
	}
	rc = pthread_create( &tid[1], NULL, direction_reset_thread, NULL );
	if ( rc != 0 ) {
		perror( "Could not create thread" );
	}
	rc = pthread_create( &tid[2], NULL, target_set_thread, NULL );
	if ( rc != 0 ) {
		perror( "Could not create thread" );
	}
	rc = pthread_create( &tid[3], NULL, target_move_thread, NULL );
	if ( rc != 0 ) {
		perror( "Could not create thread" );
	}

  while (!done) {
    pthread_mutex_lock(&mutex);
    if(updates == 0){
      printf("NO UPDATES!\n");
    }
    else if(target.x == 0 && target.y == 0 && target.z == 0){
      target.x = position.x;
      target.y = position.y;
      target.z = position.z;
    }
    updates = 0;
    printf("Position X:%f Y:%f Z:%f SPEED: %d\n", position.x, position.y, position.z, speed);
    printf("Target X:%f Y:%f Z:%f SPEED: %d\n", target.x, target.y, target.z, speed);
    printf("atstation: %d navigatingtostation: %d stationnumber: %d stationsenabled: %d atstationnumber: %d\n", atstation, navigatingtostation, stationnumber, stationsenabled, atstationnumber);
    printf("distance traveled: %d\n", distancetraveled);
    pthread_mutex_unlock(&mutex);
		usleep(1000000);
  }
	pthread_join( tid[0], NULL );

	pthread_mutex_destroy(&mutex);
  return 0;
}

void * target_thread(void * arg){
  while(1){
    pthread_mutex_lock(&mutex);
    lo_message message = lo_message_new();
    lo_message_add(message, "fffi", target.x, target.y, target.z, speed);
    lo_bundle bundle = lo_bundle_new( LO_TT_IMMEDIATE );
    lo_bundle_add_message(bundle, "/infinity/target", message);
    lo_send_bundle(infinitydriver, bundle);
    lo_bundle_free(bundle);
    lo_message_free(message);
    lo_send(ipad, "/speed", "f", (float)speed);
    pthread_mutex_unlock(&mutex);
    usleep(50000);
  }
}

void error(int num, const char *msg, const char *path)
{
    printf("liblo server error %d in path %s: %s\n", num, path, msg);
    fflush(stdout);
}

/* catch any incoming messages and display them. returning 1 means that the
 * message has not been fully handled and the server should try other methods */
int generic_handler(const char *path, const char *types, lo_arg **argv,
		    int argc, void *data, void *user_data)
{
	if(strcmp(path, UPDATE_PATH) == 0 /*|| strcmp(path, "/acc") == 0 || strcmp(path, "/ori") == 0 || strcmp(path, "/touch") == 0*/)
		return 1;

  int i;

  printf("path: <%s>\n", path);
  for (i=0; i<argc; i++) {
    printf("arg %d '%c' ", i, types[i]);
    lo_arg_pp(types[i], argv[i]);
    printf("\n");
  }
  printf("\n");
  fflush(stdout);

  return 1;
}

/* catch any incoming messages and display them. returning 1 means that the
 * message has not been fully handled and the server should try other methods */
int update_handler(const char *path, const char *types, lo_arg **argv,
		    int argc, void *data, void *user_data)
{
  pthread_mutex_lock(&mutex);
  int i;
  updates ++;
  position.x = argv[0]->f;
  position.y = argv[1]->f;
  position.z = argv[2]->f;
  lo_send(ipad, "/directdepth", "f", (position.x-2613)/(7900-2613));
  lo_send(ipad, "/direct", "ff", (5500-position.y)/(5500-2650), (position.z - 2650)/(10000-2650));
  pthread_mutex_unlock(&mutex);

  return 1;
}

int lightson_handler(const char *path, const char *types, lo_arg **argv,
		    int argc, void *data, void *user_data)
{
  pthread_mutex_lock(&mutex);
  lo_send(lightcontrol, "/lightson", "i", 1);
  pthread_mutex_unlock(&mutex);

  return 1;
}

int lightsoff_handler(const char *path, const char *types, lo_arg **argv,
		    int argc, void *data, void *user_data)
{
  pthread_mutex_lock(&mutex);
  lo_send(lightcontrol, "/lightsoff", "i", 1);
  pthread_mutex_unlock(&mutex);

  return 1;
}

int direction_handler(const char *path, const char *types, lo_arg **argv,
		    int argc, void *data, void *user_data)
{
  pthread_mutex_lock(&mutex);
  int direction = argv[0]->f;
  switch((int)direction){
  case 0:
    up = 0;
    padenabled = 0;
    break;
  case 1:
    up = 1;
    padenabled = 1;
    directenabled = 0;
    stationsenabled = 0;
    atstation = 0;
    stationnumber = 0;
    navigatingtostation = 0;
    pathenabled = 0;
    pathprogress = 0;
    break;
  case 2:
    down = 0;
    padenabled = 0;
    break;
  case 3:
    down = 1;
    directenabled = 0;
    stationsenabled = 0;
    atstation = 0;
    stationnumber = 0;
    navigatingtostation = 0;
    pathenabled = 0;
    pathprogress = 0;
    padenabled = 1;
    break;
  case 4:
    right = 0;
    padenabled = 0;
    break;
  case 5:
    right = 1;
    directenabled = 0;
    pathenabled = 0;
    pathprogress = 0;
    stationsenabled = 0;
    atstation = 0;
    stationnumber = 0;
    navigatingtostation = 0;
    padenabled = 1;
    break;
  case 6:
    left = 0;
    padenabled = 0;
    break;
  case 7:
    left = 1;
    directenabled = 0;
    pathenabled = 0;
    stationsenabled = 0;
    atstation = 0;
    stationnumber = 0;
    navigatingtostation = 0;
    pathprogress = 0;
    padenabled = 1;
    break;
  case 8:
    forward = 0;
    padenabled = 0;
    break;
  case 9:
    forward = 1;
    directenabled = 0;
    stationsenabled = 0;
    atstation = 0;
    stationnumber = 0;
    navigatingtostation = 0;
    pathenabled = 0;
    pathprogress = 0;
    padenabled = 1;
    break;
  case 10:
    backward = 0;
    padenabled = 0;
    break;
  case 11:
    backward = 1;
    directenabled = 0;
    pathenabled = 0;
    stationsenabled = 0;
    atstation = 0;
    stationnumber = 0;
    navigatingtostation = 0;
    pathprogress = 0;
    padenabled = 1;
    break;
  }
  pthread_mutex_unlock(&mutex);

}

void * direction_reset_thread(void * arg){
  while(1){
    /*pthread_mutex_lock(&mutex);
    if(forwardcommands == 0){
       forward = 0;
    }
    if(backwardcommands == 0){
       backward = 0;
    }
    if(upcommands == 0){
       up = 0;
    }
    if(downcommands == 0){
       down = 0;
    }
    if(leftcommands == 0){
       left = 0;
    }
    if(rightcommands == 0){
       right = 0;
    }
    forwardcommands = 0;
    backwardcommands = 0;
    upcommands = 0;
    downcommands = 0;
    leftcommands = 0;
    rightcommands = 0;
    pthread_mutex_unlock(&mutex);*/
    usleep(500000);
  }
}

/*Vector3 clamptobox(Vector3 target, int xmin, int xmax, int ymin, int ymax){
  return Vector3 targetl
}*/

void * target_set_thread(void * arg){
  Vector3 temp;
  Vector3 padposition;
  padposition.x = 5000;
  padposition.y = 5000;
  padposition.z = 5000;
  while(1){
    pthread_mutex_lock(&mutex);
    if(padenabled == 0){
      padposition = position;
    }
    if(up == 1 || down == 1){
      padposition.y = position.y;
    }
    if(left == 1 || right == 1){
      padposition.z = position.z;
    }
    if(forward == 1 || backward == 1){
      padposition.x = position.x;
    }
    if(!directenabled && !pathenabled && !stationsenabled){
      temp.x = 0;
      temp.y = 0;
      temp.z = 0;
      temp.x += forward;
      temp.x -= backward;
      temp.y += down;
      temp.y -= up;
      temp.z += right;
      temp.z -= left;
      if(vecLength(temp) != 0)
        temp = vecDiv(temp, vecLength(temp));
      temp = vecMult(temp, 1000);
      target = vecAdd(padposition, temp);
    }
    pthread_mutex_unlock(&mutex);
    usleep(50000);
  }
}

int emergency_handler(const char *path, const char *types, lo_arg **argv, int argc,
		 void *data, void *user_data){
  pthread_mutex_lock(&mutex);
  while(1){
    printf("EMERGENCY STOP!\n");
    lo_message message = lo_message_new();
    lo_message_add(message, NULL);
    lo_bundle bundle = lo_bundle_new( LO_TT_IMMEDIATE );
    lo_bundle_add_message(bundle, "/stop", message);
    lo_send_bundle(infinitydriver, bundle);
    lo_bundle_free(bundle);
    lo_message_free(message);
    usleep(50000);
  }
}


int direct_handler(const char *path, const char *types, lo_arg **argv, int argc,
  void *data, void *user_data){
  pthread_mutex_lock(&mutex);
  target.y = 5500-((argv[0]->f) * (5500-2650));
  target.z = 2650+((argv[1]->f) * (10000-2650));
  directenabled = 1;
  pathenabled = 0;
  pathprogress = 0;
  atstation = 0;
  stationnumber = 0;
  navigatingtostation = 0;
  pthread_mutex_unlock(&mutex);
}

int depth_handler(const char *path, const char *types, lo_arg **argv,
		    int argc, void *data, void *user_data){
  pthread_mutex_lock(&mutex);
  target.x = 2613+((argv[0]->f)*(7900-2613));
  directenabled = 1;
  pathenabled = 0;
  pathprogress = 0;
  atstation = 0;
  stationnumber = 0;
  navigatingtostation = 0;
  pthread_mutex_unlock(&mutex);
}

int save_handler(const char *path, const char *types, lo_arg **argv, int argc,
		 void *data, void *user_data){
  if(argv[0]->f == 1){
    pthread_mutex_lock(&mutex);
    storedtarget.x = position.x;
    storedtarget.y = position.y;
    storedtarget.z = position.z;
    pthread_mutex_unlock(&mutex);
  }
}

int go_handler(const char *path, const char *types, lo_arg **argv, int argc,
		 void *data, void *user_data){
  pthread_mutex_lock(&mutex);
  target = storedtarget;
  directenabled = 1;
  pathenabled = 0;
  atstation = 0;
  stationnumber = 0;
  navigatingtostation = 0;
  pathprogress = 0;
  pthread_mutex_unlock(&mutex);
}

int path_handler(const char *path, const char *types, lo_arg **argv, int argc,
		 void *data, void *user_data){
  pthread_mutex_lock(&mutex);
  printf("ENABLED\n");
  pathprogress = 0;
  directenabled = 0;
  atstation = 0;
  stationnumber = 0;
  navigatingtostation = 0;
  pathenabled = 1;
  pthread_mutex_unlock(&mutex);
}

int clear_state_handler(const char *path, const char *types, lo_arg **argv, int argc,
		 void *data, void *user_data){
  pthread_mutex_lock(&mutex);
  lo_message message = lo_message_new();
  lo_message_add(message, "i", 10100);
  lo_bundle bundle = lo_bundle_new( LO_TT_IMMEDIATE );
  lo_bundle_add_message(bundle, "/state", message);
  lo_send_bundle(infinitydriver, bundle);
  lo_bundle_free(bundle);
  lo_message_free(message);
  pthread_mutex_unlock(&mutex);
}

int load_state_handler(const char *path, const char *types, lo_arg **argv, int argc,
		 void *data, void *user_data){
  printf("LOADING!\n");
  pthread_mutex_lock(&mutex);
  lo_message message = lo_message_new();
  lo_message_add(message, "i", 13000);
  lo_bundle bundle = lo_bundle_new( LO_TT_IMMEDIATE );
  lo_bundle_add_message(bundle, "/state", message);
  lo_send_bundle(infinitydriver, bundle);
  lo_bundle_free(bundle);
  lo_message_free(message);
  pthread_mutex_unlock(&mutex);
}

int playback_handler(const char *path, const char *types, lo_arg **argv, int argc,
		 void *data, void *user_data){
  pthread_mutex_lock(&mutex);
  lo_message message = lo_message_new();
  lo_message_add(message, "i", argv[0]->i);
  lo_bundle bundle = lo_bundle_new( LO_TT_IMMEDIATE );
  lo_bundle_add_message(bundle, "/playback", message);
  lo_send_bundle(infinitydriver, bundle);
  lo_bundle_free(bundle);
  lo_message_free(message);
  pthread_mutex_unlock(&mutex);
}

int speed_handler(const char *path, const char *types, lo_arg **argv, int argc,
		 void *data, void *user_data){
  pthread_mutex_lock(&mutex);
  speed = (int)argv[0]->f;
  pthread_mutex_unlock(&mutex);
}

int station_handler(const char *path, const char *types, lo_arg ** argv, int argc,
		void *data, void *user_data){
	pthread_mutex_lock(&mutex);
	if(stationsenabled == 0){
		stationnumber = (int)argv[0]->i;
		stationsenabled = 1;
		atstation = 0;
		navigatingtostation = 0;
	}
	if(stationsenabled == 1 && atstation != 0 && atstationnumber != (int)argv[0]->i && navigatingtostation == 0){
		stationnumber = (int)argv[0]->i;
	}
	pthread_mutex_unlock(&mutex);
}

void * target_move_thread(void * arg){
  Vector3 temp;
  while(1){
    pthread_mutex_lock(&mutex);
    if(vecLength(vecSub(target,position)) < 20 && directenabled){
      directenabled = 0;
    }
    if(pathenabled == 1){
	  if(pathprogress == 0){
		  target = path[0];
		  if(vecLength(vecSub(target,position)) < 100)
		  	pathprogress++;
			//printf("Went!\n");
  	  }
  	  if(pathprogress == 1){
		  double height = 0.000627682*pow((position.z+100),2) + (-4.64484*(position.z+100)) + 12856;
		  temp.x = 5100;
		  temp.y = height;
		  temp.z = position.z+100;
		  temp = vecAdd(vecMult(vecDiv(vecSub(temp,position), vecLength(vecSub(temp,position))), 2000),position);
		  temp.x = 5100;
		  target = temp;
		  if(vecLength(vecSub(path[1], position)) < 100){
			  pathenabled = 0;
			  pathprogress = 0;
		  }
	  }
    }
    if(stationsenabled == 1){
		if(atstation == 0){
			target.x = stations[stationnumber].x;
			target.y = stations[stationnumber].y;
			target.z = stations[stationnumber].z;
			if(vecLength(vecSub(target,position)) < 100){
				atstation = 1;
				atstationnumber = stationnumber;
			}
		}
		if(stationnumber != atstationnumber && atstation == 1 && navigatingtostation == 0){
			navigatingtostation = 1;
			distancetraveled = 0;
			atstation = 0;
		}
		if(navigatingtostation == 1){
			double tempx,tempy,tempz;
			if(atstationnumber == 0){

				distancetraveled = sqrt(pow(5203 - position.x, 2) + pow(5978 - position.z, 2)) + 500;

				Vector3 temp;
				temp = stations[0];

				tempx = 0.5564552 * distancetraveled;
				tempz = 0.830877615 * distancetraveled;
				tempy = (-0.000480983*pow(distancetraveled,2))  +  (2.14882*distancetraveled);
				if(stationnumber == 1){
					temp.x = 5203 - tempx;
					temp.y = 6077 - tempy;
					temp.z = 5978 - tempz;
				}
				if(stationnumber == 2){
					temp.x = 5203 - tempx;
					temp.y = 6077 - tempy;
					temp.z = 5978 + tempz;
				}
				if(stationnumber == 3){
					temp.x = 5203 + tempx;
					temp.y = 6077 - tempy;
					temp.z = 5978 - tempz;
				}
				if(stationnumber == 4){
					temp.x = 5203 + tempx;
					temp.y = 6077 - tempy;
					temp.z = 5978 + tempz;
				}

				target = vecAdd(vecMult(vecSub(temp,position), 100), position);

				/*if(vecLength(vecSub(stations[stationnumber], position)) < 200){
					target = stations[stationnumber];
					atstation = 1;
					atstationnumber = stationnumber;
					navigatingtostation = 0;
				}*/
			}
			else{
				if(atstationnumber == 1){
					Vector3 temp;
					temp = stations[0];
					if(stationnumber == 0){
						distancetraveled = sqrt(pow(2717 - position.x, 2) + pow(2267 - position.z, 2)) + 500;

						tempx = (0.5564552 * distancetraveled) + 2717;
						tempz = (0.830877615 * distancetraveled) + 2267;
						tempy = 6077 - ((-0.000480983*pow(distancetraveled,2))  +  (2.14882*distancetraveled));
					}
					if(stationnumber == 2){
						distancetraveled = position.z + 500;

						tempx = 2717;
						tempz = distancetraveled;
						tempy = (0.000131213*pow(distancetraveled,2))  +  (-1.56878*distancetraveled) + 8959.08;
					}
					if(stationnumber == 3){
						distancetraveled = position.x + 500;

						tempx = distancetraveled;
						tempz = 2267;
						tempy = (0.000294801*pow(distancetraveled,2))  +  (-3.068*distancetraveled) + 12236.5;
					}

					temp.x = tempx;
					temp.y = tempy;
					temp.z = tempz;

					target = vecAdd(vecMult(vecSub(temp,position), 100), position);
				}

				if(atstationnumber == 2){
					Vector3 temp;
					temp = stations[0];
					if(stationnumber == 0){
						distancetraveled = sqrt(pow(2717 - position.x, 2) + pow(9689 - position.z, 2)) + 500;

						tempx = (0.5564552 * distancetraveled) + 2717;
						tempz = 9689 - (0.830877615 * distancetraveled);
						tempy = 6077-((-0.000480983*pow(distancetraveled,2))  +  (2.14882*distancetraveled));
					}
					if(stationnumber == 1){
						distancetraveled = position.z - 500;

						tempx = 2717;
						tempz = distancetraveled;
						tempy = (0.000131213*pow(distancetraveled,2))  +  (-1.56878*distancetraveled) + 8959.08;
					}
					if(stationnumber == 4){
						distancetraveled = position.x + 500;

						tempx = distancetraveled;
						tempz = 9689;
						tempy = (0.000294801*pow(distancetraveled,2))  +  (-3.068*distancetraveled) + 12236.5;
					}

					temp.x = tempx;
					temp.y = tempy;
					temp.z = tempz;

					target = vecAdd(vecMult(vecSub(temp,position), 100), position);
				}

				if(atstationnumber == 3){
					Vector3 temp;
					temp = stations[0];
					if(stationnumber == 0){
						distancetraveled = sqrt(pow(7690 - position.x, 2) + pow(2267 - position.z, 2)) + 500;

						tempx = 7690 - (0.5564552 * distancetraveled);
						tempz = 2267 + (0.830877615 * distancetraveled);
						tempy = 6077-((-0.000480983*pow(distancetraveled,2))  +  (2.14882*distancetraveled));
					}
					if(stationnumber == 1){
						distancetraveled = position.x - 500;

						tempx = distancetraveled;;
						tempz = 2267;
						tempy = (0.000294801*pow(distancetraveled,2))  +  (-3.068*distancetraveled) + 12236.5;
					}
					if(stationnumber == 4){
						distancetraveled = position.z + 500;

						tempx = 7690;
						tempz = distancetraveled;
						tempy = (0.000131213*pow(distancetraveled,2))  +  (-1.56878*distancetraveled) + 8959.08;
					}

					temp.x = tempx;
					temp.y = tempy;
					temp.z = tempz;

					target = vecAdd(vecMult(vecSub(temp,position), 100), position);
				}
				if(atstationnumber == 4){
					Vector3 temp;
					temp = stations[0];
					if(stationnumber == 0){
						distancetraveled = sqrt(pow(7690 - position.x, 2) + pow(9689 - position.z, 2)) + 500;

						tempx = 7690 - (0.5564552 * distancetraveled);
						tempz = 9689 - (0.830877615 * distancetraveled);
						tempy = 6077-((-0.000480983*pow(distancetraveled,2))  +  (2.14882*distancetraveled));
					}
					if(stationnumber == 2){
						distancetraveled = position.x - 500;

						tempx = distancetraveled;;
						tempz = 9689;
						tempy = (0.000294801*pow(distancetraveled,2))  +  (-3.068*distancetraveled) + 12236.5;
					}
					if(stationnumber == 3){
						distancetraveled = position.z - 500;

						tempx = 7690;
						tempz = distancetraveled;
						tempy = (0.000131213*pow(distancetraveled,2))  +  (-1.56878*distancetraveled) + 8959.08;
					}

					temp.x = tempx;
					temp.y = tempy;
					temp.z = tempz;

					target = vecAdd(vecMult(vecSub(temp,position), 100), position);
				}

			}
			if(vecLength(vecSub(stations[stationnumber], position)) < 400){
				target = stations[stationnumber];
				atstation = 1;
				atstationnumber = stationnumber;
				navigatingtostation = 0;
			}
		}
	}
    pthread_mutex_unlock(&mutex);
    usleep(50000);
  }
}
